{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Number of correspondences loaded =  23\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import cv2\n",
    "import pygcransac\n",
    "from time import time\n",
    "\n",
    "correspondences = np.loadtxt('../build/data/kitchen/kitchen_points.txt')\n",
    "gt_pose = np.loadtxt('../build/data/kitchen/kitchen_gt.txt')\n",
    "\n",
    "print(\"Number of correspondences loaded = \", str(len(correspondences)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def verify_pygcransac(corrs):    \n",
    "    n = len(corrs)\n",
    "    points1 = np.float32([corrs[i][0:3] for i in np.arange(n)]).reshape(-1,3)\n",
    "    points2 = np.float32([corrs[i][3:6] for i in np.arange(n)]).reshape(-1,3)\n",
    "    \n",
    "    threshold = 100.0\n",
    "    pose, mask = pygcransac.findRigidTransform(\n",
    "        np.ascontiguousarray(points1), \n",
    "        np.ascontiguousarray(points2),\n",
    "        threshold, 0.999,\n",
    "        use_sprt=True)    \n",
    "    return pose, mask\n",
    "\n",
    "def tranform_points(corrs, T):\n",
    "    n = len(corrs)\n",
    "    points1 = np.float32([corrs[i][0:3] for i in np.arange(n)]).reshape(-1,3)\n",
    "    points2 = np.float32([corrs[i][3:6] for i in np.arange(n)]).reshape(-1,3)\n",
    "    \n",
    "    transformed_corrs = np.zeros((corrs.shape[0], 6))\n",
    "\n",
    "    for i in range(n):\n",
    "        p1 = np.append(correspondences[i][:3], 1)\n",
    "        p2 = p1.dot(T)\n",
    "        transformed_corrs[i][:3] = p2[:3]\n",
    "        transformed_corrs[i][3:] = corrs[i][3:]\n",
    "    return transformed_corrs\n",
    "    \n",
    "\n",
    "def calculate_error(gt_pose, est_pose):\n",
    "    \n",
    "    R2R1 = np.dot(gt_pose[:3, :3].T, est_pose[:3, :3])\n",
    "    cos_angle = max(-1.0, min(1.0, 0.5 * (R2R1.trace() - 1.0)))\n",
    "    \n",
    "    err_R = np.arccos(cos_angle) * 180.0 / np.pi\n",
    "    err_t = np.linalg.norm(gt_pose[3, :3] - est_pose[3, :3])\n",
    "    \n",
    "    return err_R, err_t\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.003989219665527344  sec gc-ransac\n",
      "Rotation error =  4.425706159808607 °\n",
      "Translation error =  2.1672209587337785  mm\n"
     ]
    }
   ],
   "source": [
    "\n",
    "initial_T = gt_pose[:4, :]\n",
    "ground_truth_T = gt_pose[4:, :]\n",
    "\n",
    "transformed_corrs = tranform_points(correspondences, initial_T)\n",
    "\n",
    "t = time()\n",
    "\n",
    "gc_T, gc_mask = verify_pygcransac(transformed_corrs)\n",
    "print (time()-t, ' sec gc-ransac')\n",
    "\n",
    "gc_T = np.dot(initial_T, gc_T)\n",
    "    \n",
    "err_R, err_t = calculate_error(ground_truth_T, gc_T)\n",
    "\n",
    "print ('Rotation error = ', err_R, '°')\n",
    "print ('Translation error = ', err_t, ' mm')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
